%% 5、利用标定得到的r和Psi带入模型求PD的坐标xR
clear all;
Vm = [0, 0, -1];     % 灯泡方向
ur = [0, 0, 1];      % UE方向向量
ur = ur/norm(ur);    % 归一化
% k = 80;
% xr = 0.01 * PD_pos(k, :);      % 第k个数据，接收器PD真实坐标
% xr(3) = 0.085;
% z = load('z_real.txt');  % 读取由python计算得到的真实z，用于估计位置
z = readmatrix('z_real.txt');
writematrix(z, 'whatzread.csv')
% XR_init = xr + 0.1*rand(1,3); % 随机选取初始点
% XR_init = 0*rand(1,3); % 0!
XR_init = [0.35, 0.35, 0.085]; % 中心
%% 5.1、筛选视距传播
i = 0; %i----视距传播个数
for j = 1:length(Pm(:,1))
    theta(j) = acos((Pm(j, :)-xr)*ur'/(norm(xr-Pm(j, :))));%入射角度
    phi(j) = acos((xr-Pm(j, :))*Vm'/(norm(xr-Pm(j, :))));%发射角度
    if abs(theta(j)/(pi/2))<=1
        if abs(phi(j)/(pi/2))<=1
            i = i+1;
            pm(i, :) = Pm(j, :);
        end
    end
end
M = readmatrix('LED_PD_position.csv');
LED_pos = M(1:4, 2:4);
Pm = 0.01 * LED_pos;   % LED的坐标
pm = Pm;
%% 5.2、迭代估计位置和方向
k_diedai = 100;                       % 迭代次数
XR_estimate = zeros(k_diedai+1,3);    % 估计坐标预定空间
U_estimate = zeros(k_diedai+1,3);     % 估计方位预定空间
XR_estimate(1,:) = XR_init;           % 初始化位置信息
for i = 1:k_diedai
%     loss(i) = norm(XR_estimate(i,:)-xr); % 估计位置与真实位置的直线距离，仅用于计算误差画图
%     U_estimate(i,:) = z*pinv(G(XR_estimate(i,:),pm))';%1x3 % 根据真实计算得到的增益和估计得到的增益反求估计的方向向量
    U_estimate(i,:) = [0, 0, 1]; % 降维，固定U_R
    XR_estimate(i+1,:) = XR_estimate(i,:)+(z-(U_estimate(i,:))*G(XR_estimate(i,:),pm)')*(pinv(G_XR_derivation(XR_estimate(i,:),U_estimate(i,:),pm)))';
end
% u_ = U_estimate(k_diedai,:)/norm(U_estimate(k_diedai,:))  % 最终归一化方向估计结果
% xr;
x_ = XR_estimate(k_diedai+1,:);    % 最终位置估计结果
fprintf('PD的真实坐标为   %f, %f, %f\n', xr(1), xr(2), xr(3));
fprintf('迭代的初始坐标为 %f, %f, %f\n', XR_init(1), XR_init(2), XR_init(3));
fprintf('估计结果为           %f, %f, %f\n', x_(1), x_(2), x_(3));
% 5.3、画三维图以及误差曲线
figure(1)
grid on
hold on
a=plot3(Pm(:,1),Pm(:,2),Pm(:,3), '^', 'markersize', 7, 'Markerfacecolor','y', 'markeredgecolor','k');
% b=plot3(xr(:,1),xr(:,2),xr(:,3),'o', 'markersize', 7, 'Markerfacecolor','r', 'markeredgecolor','k');
b = plot3(0.35, 0.45, 0.085,'o', 'markersize', 7, 'Markerfacecolor','r', 'markeredgecolor','k');
c=plot3(x_(:,1),x_(:,2),x_(:,3),'*', 'markersize', 9, 'Markerfacecolor','b', 'markeredgecolor','k');
legend([a,c],'LED','Estimate location');
xlim([0, 0.75])
ylim([0, 0.75])
view(0,90)
figure(2)
semilogy(100*loss)
xlabel('迭代次数'); ylabel('距离误差(cm)')
%% 5.4、保存PD_estimate结果为csv
writematrix(XR_estimate, 'PDpos_record.csv')
writematrix(x_, 'PDpos_result.csv')
%% 6、相关函数 ----------------------------------------------------------------
function [G] = G(XR, Pm)
    % XR 接收器的真实(物理)坐标
    % Pm 视距内LED坐标
%     r_and_Psi_estimate_result = load('r_Psi_mean.mat'); % 用load会一起打包
    M = readmatrix('r_Psi_result.csv');
    rr = M(1,:);
    PP = M(2,:);
    
    Vm = [0, 0, -1];    % 代表LED的朝向，这里默认所有灯泡方向都垂直于地面
    num = length(Pm(:, 1));     % 视距内LED的个数
    for i = 1:num
        R = norm(XR-Pm(i,:));             % 分母, 真实位置与LED的直线距离
        r = rr(i);
        Psi_R = PP(i);
        G(i, :) = Psi_R*((r+1)*(Vm*(XR-Pm(i,:))').^r)*(XR-Pm(i,:))/R^(r+3); % 公式(5)和(6)之间的g_m(x_R)
    end
end
function [g_XR_derivation]=G_XR_derivation(XR_estimate,U_estimate,Pm)
    % g关于xR的一阶导数
    g_XR_derivation = zeros(length(Pm(:,1)),3);%预定存贮空间
    for i = 1:length(Pm(:,1))
        em = (XR_estimate-Pm(i,:))/norm(XR_estimate-Pm(i,:));%发射灯光方向
        Vm = [0,0,-1];    %代表灯泡的朝向，这里默认所有灯泡方向都垂直于地面    
%         r_and_Psi_estimate_result = load('r_Psi_mean.mat');
        M = readmatrix('r_Psi_result.csv');
        rr = M(1,:);
        PP = M(2,:);
    
        r = rr(i);
        Psi_R = PP(i);

        G1=-(r+1)*(r+3)*(em*Vm')^(r)*(em*U_estimate')*em;
        G2=(r+1)*(em*Vm')^(r)*U_estimate;
        G3=r*(r+1)*(em*U_estimate')*Vm;
        G4=norm(XR_estimate-Pm(i,:)).^3;
        g_XR_derivation(i,:)=Psi_R*(G1+G2+G3)/G4;
    end
end
% -------------------------------------------------------------------------